// accepts commands from the terminal to control the compas
//
// Calibrate: 1
// Stop calibration: 2
// Read: 3


#include <Wire.h>

#define INVALID 0x00
#define CALIBRATE 0xC0
#define STOP_CALIBRATE 0xC1
#define READ 0x31

#define WIPE 0x00

/**
*
*
*/
void setup()
{
  Serial.begin(9600);
  Serial1.begin(9600);
}

/**
*
*
*/
byte translate(byte command)
{ 
  byte translation = INVALID;
  switch (command)
 {
   case '1':
     translation = CALIBRATE;
     break;
   case '2':
     translation = STOP_CALIBRATE;
     break;
   case '3':
     translation = READ;
     break;
   default:
     translation = INVALID;
     break;
 } 
 
 return translation;
}

/**
*
*
*/
byte forwardCommand()
{
  byte incomming;
  if (Serial.available())
  {
    incomming = Serial.read();
    
    incomming = translate(incomming);
    
    Serial.print("sent command: ");
    Serial.println(incomming);
    Serial1.write(incomming);
  }
  
  return incomming;
}

/**
*
*
*/
byte forwardResult()
{
  byte incomming = INVALID;
  
  while (Serial1.available())
  {
    incomming = Serial1.read();
    
    Serial.print("recieved response: ");
    Serial.println(incomming);
  }
  
  return incomming;
}

/**
*
*
*/
byte getResult()
{
  byte incomming = INVALID;
  
  if (Serial1.available())
  {
    incomming = Serial1.read();
  }
  
  return incomming;
}

/**
*
*
*/
int translateRead(byte response, byte wipe)
{
  static byte byteNum = 0;
  static float angle  = 0;
  float result        = INVALID;
  
  if (WIPE == wipe)
  {
    byteNum = 0;
    angle   = 0;
    return WIPE; 
  }
  
  if (byteNum == 0 && response != 0x0D)
  {
    return result;
  }
  
  byteNum++;
  
  switch (byteNum) {
      case 3:
        angle += (response & 0x03)*100;
        break;
      case 4:
        angle += (response & 0x0F)*10;
        break;
      case 5:
        angle += (response & 0x0F);
        break;
      case 7:
        angle += (response & 0x0F)*(0.1);
        result = angle; 
        break;
      default:
        break;
  }
  
  return result;
}

void loop()
{ 
  byte response   = INVALID;
  float angle     = INVALID;
  byte translated = forwardCommand();
  
  if (READ == translated)
  { 
    while (true)
    {
      response = getResult();
      angle    = translateRead(response, !WIPE);
      
      if (angle != INVALID)
      {
        translateRead(INVALID, WIPE);
        Serial.print("received angle: ");
        Serial.println(angle);
        break;
      }
    }
  } 
  else 
  {
    forwardResult(); 
  }
  
}
